#include "joystick.h"

void Joystick::attach (int pin_x, int pin_y, void (*on_attached)(void)) {
    this->pin_x = pin_x;
    this->pin_y = pin_y;

    init ();
    if (NULL != on_attached) {
        on_attached ();
    }
}

void Joystick::init () {
#ifdef __JO_DBG_
    long now = millis ();
#endif
    int sum_x = 0, sum_y = 0, k = 0;
    for (k = 0; k < 5; k ++) {
        sum_x += analogRead (pin_x);
        sum_y += analogRead (pin_y);
#ifdef __JO_DBG_
        for (int j = 0; j < 3; j ++) {
            digitalWrite (13, HIGH);
            delay (100);
            digitalWrite (13, LOW);
            delay (100);
        }
#else
        delay (200);
#endif
    }
    bx = x = sum_x / 5;
    by = y = sum_y / 5;
    vx0 = vx = _map2 (x, bx);
    vy0 = vy = _map2 (y, by);

#ifdef __JO_DBG_
    Serial.print ("vx0 = "); Serial.print (vx);
    Serial.print (",vy0 = "); Serial.println (vy);

    Serial.print ("boot up time: ");
    Serial.println (millis () - now);
#endif
}

void Joystick::check (void (*on_change)(uint32_t value)) {
    x  = analogRead (pin_x);
    y  = analogRead (pin_y);

    vx = _map2(x, bx);
    vy = _map2(y, by);
    
    if (abs(vx0 - vx) >= 5 || abs(vy0 - vy) >= 5) {
/*
        analogWrite (5, vx);
        analogWrite (6, vy);
*/
        vx0 = vx;
        vy0 = vy;
        data.en_x = x <= bx ? 0 : 1;
        data.en_y = y <= by ? 0 : 1;
        data.x    = vx;
        data.y    = vy;

        uint32_t en = (data.en_x << 1) | data.en_y;
        uint32_t n = (en << 16) | (data.x << 8) | data.y;
#ifdef __JO_DBG_
        Serial.print ("x = ");       Serial.print (x);
        Serial.print (", y = ");     Serial.print (y);
        Serial.print (", en_x = ");  Serial.print (data.en_x);
        Serial.print (", vx = ");    Serial.print (vx);
        Serial.print (", en_yy = "); Serial.print (data.en_y);
        Serial.print (", vy = ");    Serial.print (vy);        
        Serial.print (", value = "); Serial.println (n, HEX);
#endif
        if (NULL != on_change) {
            on_change (n);
        }
    }
}

int Joystick::_map2 (int n, int base) {
    int z;
    if (n <= base) {
        z = map (n, 0, base, 2550, 0);
    } else {
        z = map (n, base, 1023, 0, 2550);
    }

    return z;
}
